// actoin: /navigate_to_pose [nav2_msgs/action/NavigateToPose]

/*
#goal definition
geometry_msgs/PoseStamped pose
    std_msgs/Header header
        builtin_interfaces/Time stamp
            int32 sec
            uint32 nanosec
        string frame_id
    Pose pose
        Point position
            float64 x
            float64 y
            float64 z
        Quaternion orientation
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1
string behavior_tree
---
#result definition
std_msgs/Empty result
---
#feedback definition
geometry_msgs/PoseStamped current_pose
    std_msgs/Header header
        builtin_interfaces/Time stamp
            int32 sec
            uint32 nanosec
        string frame_id
    Pose pose
        Point position
            float64 x
            float64 y
            float64 z
        Quaternion orientation
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1
builtin_interfaces/Duration navigation_time
    int32 sec
    uint32 nanosec
builtin_interfaces/Duration estimated_time_remaining
    int32 sec
    uint32 nanosec
int16 number_of_recoveries
float32 distance_remaining
 */

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

using nav2_msgs::action::NavigateToPose;
using std::placeholders::_1;
using std::placeholders::_2;

class NavToPose : public rclcpp::Node
{
private:
    rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr client_;

public:
    NavToPose() : Node("nav_to_pose_node")
    {
        client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
    }
    void send_goal()
    {
        if (!client_->wait_for_action_server())
        {
            RCLCPP_INFO(this->get_logger(), "连接服务器失败！");
        }
        auto goal = NavigateToPose::Goal();
        goal.pose.header.stamp = this->get_clock()->now();
        goal.pose.header.frame_id = "map";
        goal.pose.pose.position.x = 1.0;
        goal.pose.pose.position.y = 1.0;
        goal.pose.pose.position.z = 0.0;
        goal.pose.pose.orientation.w = 1.0;
        rclcpp_action::Client<NavigateToPose>::SendGoalOptions options;
        options.feedback_callback = std::bind(&NavToPose::FeedBack_CallBack, this, _1, _2);
        options.goal_response_callback = std::bind(&NavToPose::Goal_Response_CallBack, this, _1);
        options.result_callback = std::bind(&NavToPose::Result_CallBack, this, _1);
        client_->async_send_goal(goal, options);
    }
    // GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>
    void Goal_Response_CallBack(rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_INFO(this->get_logger(), "服务器拒绝！");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "服务器接收！");
        }
    }
    // FeedbackCallback =std::function<void (typename ClientGoalHandle<ActionT>::SharedPtr,const std::shared_ptr<const Feedback>)>;
    void FeedBack_CallBack(rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr goal_handle, const std::shared_ptr<const NavigateToPose::Feedback> feedback)
    {
        (void)goal_handle;
        auto feedback_value = NavigateToPose::Feedback();
        feedback_value.navigation_time.sec = feedback->navigation_time.sec;
        feedback_value.current_pose.pose.position.x = feedback->current_pose.pose.position.x;
        feedback_value.current_pose.pose.position.y = feedback->current_pose.pose.position.y;
        feedback_value.current_pose.pose.orientation.w = feedback->current_pose.pose.orientation.w;
        feedback_value.distance_remaining = feedback->distance_remaining;
        RCLCPP_INFO(this->get_logger(),
                    "导航运行时间：%d,x轴坐标:%.2f,y轴坐标:%.2f,转向角:%.2f,剩余距离:%.2f",
                    feedback_value.navigation_time.sec,
                    feedback_value.current_pose.pose.position.x,
                    feedback_value.current_pose.pose.position.y,
                    feedback_value.current_pose.pose.orientation.w,
                    feedback_value.distance_remaining);
    }
    // ResultCallback = std::function<void (const WrappedResult & result)>;
    void Result_CallBack(const rclcpp_action::ClientGoalHandle<NavigateToPose>::WrappedResult &result)
    {

        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "导航成功!");
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_INFO(this->get_logger(), "导航被中断!");
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_INFO(this->get_logger(), "导航被取消!");
            break;
        default:
            RCLCPP_INFO(this->get_logger(), "导航遇到未知异常!");
            break;
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<NavToPose>();
    node->send_goal();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
